#pragma once

#include <opencv2\opencv.hpp>

class AlignPage
{
public:
	static void GetVertex(cv::Vec2f vertex[4], cv::Size imgsz, cv::Matx23f warpH)
	{
		vertex[0] = warpH*cv::Vec3f(0, 0, 1);
		vertex[1] = warpH*cv::Vec3f(0, imgsz.height - 1, 1);
		vertex[2] = warpH*cv::Vec3f(imgsz.width - 1, 0, 1);
		vertex[3] = warpH*cv::Vec3f(imgsz.width - 1, imgsz.height - 1, 1);
	}

	static cv::Mat DistanceToEdge(cv::Size size, int type = CV_32SC1)
	{
		cv::Mat m(size, type);
		for (int i = 0; i < size.height; i++)
		{
			int disty = size.height - 1 - i;
			if (disty > i)
				disty = i;
			for (int j = 0; j < size.width; j++)
			{
				int distx = size.width - 1 - j;
				if (distx > j)
					distx = j;
				m.at<float>(i, j) = (distx < disty ? distx : disty);
			}
		}
		return m;
	}


	static cv::Mat FeatherFusion(cv::Size size, const std::vector<cv::Mat> &warp_matrix, const std::vector<cv::Mat> &image)
	{
		cv::Mat weightwsum(size, CV_32FC(image[0].channels()), cv::Scalar(0));
		cv::Mat valuesum(size, CV_32FC(image[0].channels()), cv::Scalar(0));

		for (size_t i = 0; i < image.size(); i++)
		{
			cv::Mat weight = DistanceToEdge(image[i].size(), CV_32FC1);
			cv::merge(std::vector<cv::Mat>(image[i].channels(), weight), weight);
			cv::Mat weight_warp;
			cv::warpAffine(weight, weight_warp, warp_matrix[i], size);
			weightwsum += weight_warp;
			weight_warp.release();
			cv::Mat image32f;
			image[i].convertTo(image32f, CV_32FC(image[i].channels()));
			image32f = image32f.mul(weight);
			weight.release();
			cv::Mat image_warp;
			cv::warpAffine(image32f, image_warp, warp_matrix[i], size);
			image32f.release();

			valuesum += image_warp;
		}

		cv::divide(valuesum, weightwsum, valuesum);
		cv::Mat value;
		valuesum.convertTo(value, image[0].type());
		return value;
	}

	static void AlignImageStrip0(std::vector<cv::Mat> &warp_matrix, const std::vector<cv::Mat> &image)
	{
		for (size_t i = 1; i < image.size(); i++)
		{
			double cc = cv::findTransformECC(image[i], image[i - 1], warp_matrix[i], cv::MOTION_EUCLIDEAN);			
		}
	}

	static cv::Size FixWarp(std::vector<cv::Mat> &warp_matrix, const std::vector<cv::Mat> &image)
	{

		std::vector<cv::Vec2f> vertices;
		cv::Vec2f vertex[4];

		GetVertex(vertex, image[0].size(), warp_matrix[0]);
		vertices.insert(vertices.end(), vertex, vertex + 4);

		for (size_t i = 1; i < image.size(); i++)
		{			
			cv::Mat warp_matrixiEX;
			cv::Mat lastRow = (cv::Mat_<float>(1, 3) << 0, 0, 1);
			cv::vconcat(warp_matrix[i], lastRow, warp_matrixiEX);
			warp_matrix[i] = warp_matrix[i - 1] * warp_matrixiEX;
			GetVertex(vertex, image[i].size(), warp_matrix[i]);
			vertices.insert(vertices.end(), vertex, vertex + 4);
		}

		cv::Rect rc = cv::boundingRect(vertices);
		cv::Mat warp_matrix_base = (cv::Mat_<float>(2, 3) << 0, 0, -rc.x, 0, 0, -rc.y);

		for (size_t i = 0; i < image.size(); i++) warp_matrix[i] = warp_matrix[i] + warp_matrix_base;

		return rc.size();
	}

	static cv::Size AlignImageStrip(std::vector<cv::Mat> &warp_matrix, const std::vector<cv::Mat> &image)
	{
		AlignImageStrip0(warp_matrix, image);
		return FixWarp(warp_matrix, image);
		std::vector<cv::Vec2f> vertices;
		cv::Vec2f vertex[4];

		GetVertex(vertex, image[0].size(), warp_matrix[0]);
		vertices.insert(vertices.end(), vertex, vertex + 4);

		for (size_t i = 1; i < image.size(); i++)
		{
			double cc = cv::findTransformECC(image[i], image[i - 1], warp_matrix[i], cv::MOTION_EUCLIDEAN);
			cv::Mat warp_matrixiEX;
			cv::Mat lastRow = (cv::Mat_<float>(1, 3) << 0, 0, 1);
			cv::vconcat(warp_matrix[i], lastRow, warp_matrixiEX);
			warp_matrix[i] = warp_matrix[i - 1] * warp_matrixiEX;
			GetVertex(vertex, image[i].size(), warp_matrix[i]);
			vertices.insert(vertices.end(), vertex, vertex + 4);
		}

		cv::Rect rc = cv::boundingRect(vertices);
		cv::Mat warp_matrix_base = (cv::Mat_<float>(2, 3) << 0, 0, -rc.x, 0, 0, -rc.y);

		for (size_t i = 0; i < image.size(); i++) warp_matrix[i] = warp_matrix[i] + warp_matrix_base;

		return rc.size();
	}

	static void ScaleParam(cv::Size &size, std::vector<cv::Mat> &warp_matrix, double scale)
	{
		size.height /= scale;
		size.width /= scale;

		for (size_t i = 0; i < warp_matrix.size(); i++)
		{
			warp_matrix[i].at<float>(0, 2) /= scale;
			warp_matrix[i].at<float>(1, 2) /= scale;
		}
	}

	static cv::Mat DownScale(const cv::Mat &input, double scale, int smoothRadius)
	{
		cv::Mat output;
		cv::blur(input, output, cv::Size(2 * smoothRadius + 1, 2 * smoothRadius + 1));
		cv::resize(output, output, cv::Size(), scale, scale, cv::INTER_CUBIC);
		return output;
	}
	
	static cv::Mat GuessWarp(cv::Point from, cv::Point to, double scale)
	{
		return (cv::Mat_<float>(2, 3) << 1, 0, scale*(to.x - from.x), 0, 1, scale*(to.y - from.y));
	}

	static cv::Mat GuessWarp(cv::Point from1, cv::Point to1, cv::Point from2, cv::Point to2, double scale)
	{
		cv::Point2d f1d(scale*from1.x, scale*from1.y);
		cv::Point2d t1d(scale*to1.x, scale*to1.y);
		cv::Point2d f2d(scale*from2.x, scale*from2.y);
		cv::Point2d t2d(scale*to2.x, scale*to2.y);

		cv::Matx44d A(f1d.x, -f1d.y, 1, 0,
			f1d.y, f1d.x, 0, 1,
			f2d.x, -f2d.y, 1, 0,
			f2d.y, f2d.x, 0, 1
			);

		cv::Vec4d B(t1d.x, t1d.y, t2d.x, t2d.y);

		cv::Vec4d X = A.inv()*B;

		double theta = atan2(X[1], X[0]);

		return (cv::Mat_<float>(2, 3) << cos(theta), -sin(theta), X[2], sin(theta), cos(theta), X[3]);
	}

	static void test()
	{
		std::vector<cv::Mat> m(3);
		double scale = 0.15;

		int sr = 5;

		m[0] = cv::imread("IMG_0183.png");
		m[1] = cv::imread("IMG_0184.png");
		m[2] = cv::imread("IMG_0185.png");

		std::vector<cv::Mat> mg(m.size());
		for (size_t i = 0; i < m.size(); i++)
		{
			cv::cvtColor(DownScale(m[i], scale, sr), mg[i], CV_BGR2GRAY);
		}

		std::vector<cv::Mat> warp_matrix(mg.size());

		warp_matrix[0] = (cv::Mat_<float>(2, 3) << 1, 0, 0, 0, 1, 0);

		cv::Point p1(1984, 7234);
		cv::Point p0(2056, 1177);

		warp_matrix[1] = GuessWarp(p1, p0, scale);

		cv::Point p12(2304, 853);
		cv::Point p22(2262, 7805);

		cv::Point p12a(5729, 1271);
		cv::Point p22a(5697, 8092);

		warp_matrix[2] = GuessWarp(p22, p12, p22a, p12a, scale);

		cv::Size fsz = AlignImageStrip(warp_matrix, mg);

		ScaleParam(fsz, warp_matrix, scale);

		for (size_t i = 0; i < mg.size(); i++)	mg[i].release();

		std::cout << "fsz=" << fsz << "\n";
		std::cout << "warp_matrix[0]=" << warp_matrix[0] << "\n";
		std::cout << "warp_matrix[1]=" << warp_matrix[1] << "\n";
		std::cout << "warp_matrix[2]=" << warp_matrix[2] << "\n";

		cv::Mat mf;
#if 1				
		mf = FeatherFusion(fsz, warp_matrix, m);
#else
		std::vector<cv::Mat> mch;
		cv::split(cv::imread("IMG_0005.tif"), mch);
		m[0] = mch[0].clone();

		//std::vector<cv::Mat> mch;
		cv::split(cv::imread("IMG_0006.tif"), mch);
		m[1] = mch[0].clone();

		//std::vector<cv::Mat> mch;
		cv::split(cv::imread("IMG_0007.tif"), mch);
		m[2] = mch[0].clone();
		for (size_t i = 0; i < mch.size(); i++)	mch[i].release();
		//cv::imwrite("mfb.png", FeatherFusion(fsz, warp_matrix, m));

		mg[0]=FeatherFusion(fsz, warp_matrix, m);

		//std::vector<cv::Mat> mch;
		cv::split(cv::imread("IMG_0005.tif"), mch);
		m[0] = mch[1].clone();

		//std::vector<cv::Mat> mch;
		cv::split(cv::imread("IMG_0006.tif"), mch);
		m[1] = mch[1].clone();

		//std::vector<cv::Mat> mch;
		cv::split(cv::imread("IMG_0007.tif"), mch);
		m[2] = mch[1].clone();
		for (size_t i = 0; i < mch.size(); i++)	mch[i].release();
		//cv::imwrite("mfg.png", FeatherFusion(fsz, warp_matrix, m));

		mg[1] = FeatherFusion(fsz, warp_matrix, m);

		//std::vector<cv::Mat> mch;
		cv::split(cv::imread("IMG_0005.tif"), mch);
		m[0] = mch[2].clone();

		//std::vector<cv::Mat> mch;
		cv::split(cv::imread("IMG_0006.tif"), mch);
		m[1] = mch[2].clone();

		//std::vector<cv::Mat> mch;
		cv::split(cv::imread("IMG_0007.tif"), mch);
		m[2] = mch[2].clone();

		for (size_t i = 0; i < mch.size(); i++)	mch[i].release();
		//cv::imwrite("mfr.png", FeatherFusion(fsz, warp_matrix, m));

		mg[2] = FeatherFusion(fsz, warp_matrix, m);

		for (size_t i = 0; i < m.size(); i++)	m[i].release();


		cv::merge(mg, mf);
#endif
		std::cout << "save\n";

		std::vector<int> compression_params;
		compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION);
		compression_params.push_back(9);

		cv::imwrite("mf.png", mf, compression_params);
	}


	static void test2(const std::vector<std::string> &path)
	{
		std::vector<cv::Mat> m(2);
		double scale = 0.15;

		int sr = 5;

		std::vector<cv::Mat> mg(m.size());
		for (size_t i = 0; i < m.size(); i++)
		{
			m[i] = cv::imread(path[i]);
			cv::Mat md = DownScale(m[i], scale, sr);
			std::cout << "1\n";
			m[i].release();
			cv::cvtColor(md, mg[i], CV_BGR2GRAY);
			std::cout << "2\n";
			md.release();
		}

		std::vector<cv::Mat> warp_matrix(mg.size());

		warp_matrix[0] = (cv::Mat_<float>(2, 3) << 1, 0, 0, 0, 1, 0);

		//cv::Point p1(2503, 10225);

		//cv::Point p0(2379, 3361);

		cv::Point p1(7331, 10554);

		cv::Point p0(6934, 3852);

		warp_matrix[1] = GuessWarp(p1, p0, scale);

		cv::Size fsz = AlignImageStrip(warp_matrix, mg);

		ScaleParam(fsz, warp_matrix, scale);

		std::cout << "fsz=" << fsz << "\n";
		std::cout << "warp_matrix[0]=" << warp_matrix[0] << "\n";
		std::cout << "warp_matrix[1]=" << warp_matrix[1] << "\n";
		double a0 = atan2(warp_matrix[0].at<float>(3), warp_matrix[0].at<float>(0));
		double a1 = atan2(warp_matrix[1].at<float>(3), warp_matrix[1].at<float>(0));

		cv::Vec3d xya21(warp_matrix[1].at<float>(2) - warp_matrix[0].at<float>(2),
			warp_matrix[1].at<float>(5) - warp_matrix[0].at<float>(5),
			a1 - a0);

	

		for (size_t i = 0; i < mg.size(); i++)
		{
			mg[i].release();
			m[i] = cv::imread(path[i]);
		}

		double err = AvgError(m[0], m[1], xya21);

		std::cout << err << "\n";

		FineTune(m[0], m[1], xya21, cv::Vec3d(10, 10, 0.1));

		cv::Mat mf;

		mf = FeatherFusion(fsz, warp_matrix, m);

		std::vector<int> compression_params;
		compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION);
		compression_params.push_back(3);

		cv::imwrite("mf1.png", mf, compression_params);
	}



	static cv::Mat GrayScale(const cv::Mat &m)
	{
		cv::Mat out;
		cv::normalize(m, out, 0, 255, cv::NORM_MINMAX, CV_8U);
		return out;
	}

	static cv::Mat ColorMap(const cv::Mat &m)
	{
		if (m.depth() != CV_8U)
			return ColorMap(GrayScale(m));
		cv::Mat out;
		// Apply Histogram Equalization
		cv::equalizeHist(m, out);
		cv::applyColorMap(out, out, cv::COLORMAP_JET);
		return out;
	}


	static double AvgError(const cv::Mat &m1, const cv::Mat &mask1, const cv::Mat &m2, const cv::Mat &mask2, cv::Vec3d &xya2To1)
	{
		double a = xya2To1[2];
		cv::Mat tf = (cv::Mat_<double>(2, 3) << cos(a), -sin(a), xya2To1[0], sin(a), cos(a), xya2To1[1]);

		cv::Mat m2_warp;
		cv::warpAffine(m2, m2_warp, tf, m1.size());

		cv::Mat mask_warp;
		cv::warpAffine(mask2, mask_warp, tf, m1.size(), cv::INTER_NEAREST, cv::BORDER_CONSTANT, cv::Scalar::all(0));

		cv::bitwise_and(mask1, mask_warp, mask_warp);

		cv::bitwise_not(mask_warp, mask_warp);

		cv::Mat m1_in=m1.clone();
		
		m1_in.setTo(cv::Scalar::all(0), mask_warp);
		m2_warp.setTo(cv::Scalar::all(0), mask_warp);

		cv::Mat df;
		cv::absdiff(m1_in, m2_warp, df);

		//cv::namedWindow("df", cv::WINDOW_NORMAL);
		//cv::imshow("df", ColorMap(df));
		//cv::waitKey(1);

		double sum = 0;
		size_t nPoint = 0;
		for (size_t i = 0; i < df.total(); i++)
		{
			if (mask_warp.at<uchar>(i) == 0)
			{
				nPoint++;
				cv::Vec3b df1 = df.at<cv::Vec3b>(i);
				sum += cv::norm(df1);
			}
		}

		m1_in.release();
		df.release();
		mask_warp.release();
		m2_warp.release();

		if (nPoint > 0)
			return sum / nPoint;
		return -1;
	}

	static double AvgError(const cv::Mat &m1, const cv::Mat &m2, cv::Vec3d &xya2To1)
	{
		cv::Mat mask1(m1.size(), CV_8UC1, cv::Scalar(255));
		cv::Mat mask2(m2.size(), CV_8UC1, cv::Scalar(255));
		double err = AvgError(m1, mask1, m2, mask2, xya2To1);
		mask1.release();
		mask2.release();
		return err;
	}

	static void FineTune(cv::Mat &m1, cv::Mat &m2, cv::Vec3d &xya, cv::Vec3d &dxya)
	{
		double minError = AvgError(m1, m2, xya);
		cv::Vec3d minxya = xya;
		cv::Vec3d d = -dxya;
		for (d[0] = -dxya[0]; d[0] <= dxya[0]; d[0] += 0.5)
		{
			for (d[1] = -dxya[1]; d[1] <= dxya[1]; d[1] += 0.5)
			{
				for (d[2] = -dxya[2]; d[2] <= dxya[2]; d[2] += 1e-2)
				{
					cv::Vec3d xya1 = xya + d;
					double error= AvgError(m1, m2, xya1);
					if (error > 0 && error < minError)
					{
						minError = error;
						minxya = xya1;
						std::cout << xya1 << "\t" << error << "\n";
					}
					else
					{
						std::cout << xya1 << "\n";
					}
				}
			}
		}

		std::cout << "min loc at" << minxya << "\t" << minError << "\n";
	}




	static cv::Vec2d Offset2(const cv::Mat &fromImage, const cv::Mat &toImage, cv::Point tlFrom, cv::Point tlTo)
	{
		cv::Rect roiFrom(tlFrom, fromImage.size());
		cv::Rect roiTo(tlTo, toImage.size());

		cv::Rect roiAnd = roiFrom & roiTo;

		if (roiAnd.width > 0 && roiAnd.height > 0)
		{
			cv::Rect roiAndFrom(roiAnd.x - roiFrom.x, roiAnd.y - roiFrom.y, roiAnd.width, roiAnd.height);
			cv::Rect roiAndTo(roiAnd.x - roiTo.x, roiAnd.y - roiTo.y, roiAnd.width, roiAnd.height);
			cv::Point2d df = cv::phaseCorrelate(fromImage(roiAndFrom), toImage(roiAndTo));
			return cv::Vec2d(roiFrom.x - roiTo.x + df.x, roiFrom.y - roiTo.y + df.y);
		}

		return cv::Vec2d(roiTo.x - roiFrom.x, roiTo.y - roiFrom.y);
	}

	static cv::Vec2d Offset2(const cv::Mat &fromImage, const cv::Mat &toImage)
	{
		cv::Vec2d df = Offset2(fromImage, toImage, cv::Point(0, 0), cv::Point(0, 0));
		if (df[0] != 0 || df[1] != 0)
		{
			//std::cout << df << "\n";
			df = Offset2(fromImage, toImage, cv::Point(std::round(df[0]), std::round(df[1])), cv::Point(0, 0));
			//std::cout << df << "\n";
		}
		return df;
	}

	static bool FusionOffset2(cv::Mat &outImage, const cv::Mat &inImage1, const cv::Mat &inImage2)
	{
		if (inImage1.channels() != inImage2.channels())
			return false;

		cv::Mat inImage1f;
		if (inImage1.channels() != 1)
		{
			cv::Mat gray;
			cv::cvtColor(inImage1, gray, CV_BGR2GRAY);
			gray.convertTo(inImage1f, CV_32F);
		}
		else
		{
			inImage1.convertTo(inImage1f, CV_32F);
		}

		cv::Mat inImage2f;
		if (inImage2.channels() != 1)
		{
			cv::Mat gray;
			cv::cvtColor(inImage2, gray, CV_BGR2GRAY);
			gray.convertTo(inImage2f, CV_32F);
		}
		else
		{
			inImage2.convertTo(inImage2f, CV_32F);
		}

		cv::Vec2d df = Offset2(inImage1f, inImage2f);

		if (df[0] == 0 && df[1] == 0)
			return false;

		std::vector<cv::Mat> warp_matrix(2);

		warp_matrix[0] = (cv::Mat_<float>(2, 3) << 1, 0, 0, 0, 1, 0);
		warp_matrix[1] = (cv::Mat_<float>(2, 3) << 1, 0, -df[0], 0, 1, -df[1]);

		std::vector<cv::Mat> ml(2);
		ml[0] = inImage1;
		ml[1] = inImage2;

		cv::Size fsz=FixWarp(warp_matrix, ml);
		outImage = FeatherFusion(fsz, warp_matrix, ml);
		return true;
	}


	static cv::Mat FeatherFusion2(const cv::Mat &from, const cv::Mat &to, cv::Vec3d xya)
	{
		std::vector<cv::Mat> warp_matrix(2);

		warp_matrix[0] = (cv::Mat_<float>(2, 3) << 1, 0, 0, 0, 1, 0);
		warp_matrix[1] = (cv::Mat_<float>(2, 3) << cos(xya[2]), -sin(xya[2]), xya[0], sin(xya[2]), cos(xya[2]), xya[1]);

		std::vector<cv::Mat> ml(2);
		ml[0] = to;
		ml[1] = from;

		cv::Size fsz = FixWarp(warp_matrix, ml);
		return FeatherFusion(fsz, warp_matrix, ml);
	}
 

	template<typename T>
	static T MinT(T a, T b)
	{
		return (a < b ? a : b);
	}

	static double findrotate(const cv::Mat &from, cv::Point centerFrom, const cv::Mat &to, cv::Point centerTo, bool useLog = false)
	{
		int rFrom = MinT(MinT(MinT(centerFrom.x, from.cols - centerFrom.x), centerFrom.y), from.rows - centerFrom.y);
		int rTo = MinT(MinT(MinT(centerTo.x, to.cols - centerTo.x), centerTo.y), to.rows - centerTo.y);
		int r = MinT(rFrom, rTo) - 1;

		std::cout << r << "\n";

		cv::Mat fromf = from(cv::Rect(centerFrom.x - r, centerFrom.y - r, 2 * r + 1, 2 * r + 1));

		cv::Mat tof = to(cv::Rect(centerTo.x - r, centerTo.y - r, 2 * r + 1, 2 * r + 1));

		cv::Point center(r + 1, r + 1);

		cv::Mat fromfp;
		if (useLog)
			cv::logPolar(fromf, fromfp, centerFrom, 2 * r / log(r), cv::INTER_CUBIC + cv::WARP_FILL_OUTLIERS);
		else
			cv::linearPolar(fromf, fromfp, center, r, cv::INTER_CUBIC + cv::WARP_FILL_OUTLIERS);

		//cv::imshow("fromfp", ColorMap(fromfp));
		//cv::waitKey(1);

		//std::cout << fromfp.size() << "\n";

		cv::Mat tofp;
		if (useLog)
			cv::logPolar(tof, tofp, centerTo, 2 * r / log(r), cv::INTER_CUBIC + cv::WARP_FILL_OUTLIERS);
		else
			cv::linearPolar(tof, tofp, center, r, cv::INTER_CUBIC + cv::WARP_FILL_OUTLIERS);

		//std::cout << tofp.size() << "\n";
		//
		//cv::imshow("tofp", ColorMap(tofp));
		//cv::waitKey(1);



		cv::Point2d df = cv::phaseCorrelate(fromfp, tofp);

		std::cout << df << "\n";

		double rad = df.y*CV_2PI / fromfp.rows;

		return rad;
	}


	static void irotate(double rad, int win, int hin, int &wout, int &hout, cv::Matx23d &tf)
	{
		double arad = fabs(rad);//-PI/2<rad<PI/2
		cv::Matx22d A(cos(arad), sin(arad), sin(arad), cos(arad));
		cv::Vec2d sz = A.solve(cv::Vec2d(win - 1, hin - 1), cv::DECOMP_LU);
		//std::cout << sz << "\n";
		wout = sz[0] + 1;
		hout = sz[1] + 1;

		cv::Matx22d rm(cos(rad), -sin(rad), sin(rad), cos(rad));
		cv::Vec2d tt = cv::Vec2d(wout - 1, hout - 1) - rm*cv::Vec2d(win - 1, hin - 1);

		tf = cv::Matx23d(cos(rad), -sin(rad), tt[0] / 2, sin(rad), cos(rad), tt[1] / 2);
		//std::cout << tf << "\n";
	}

	static void irotate(double rad, const cv::Mat& from, cv::Mat &to, cv::Matx23d &tf)
	{
		cv::Size tosz;
		irotate(rad, from.cols, from.rows, tosz.width, tosz.height, tf);
		cv::warpAffine(from, to, tf, tosz, cv::INTER_CUBIC, cv::BORDER_CONSTANT, cv::Scalar::all(0));
	}


	static double irerr(double rad, const cv::Mat& from, const cv::Mat &to, cv::Vec3d &w)
	{
		cv::Mat fromf2;
		cv::Matx23d ttf;
		irotate(rad, from, fromf2, ttf);
		cv::Vec2d dd = AlignPage::Offset2(fromf2, to);
		w = cv::Vec3d(dd[0] + ttf(0, 2), dd[1] + ttf(1, 2), rad);
		double err = AlignPage::AvgError(to, from, w);
		std::cout << w << "\t";
		std::cout << err << "\n";
		return err;
	}




	static double tune(const cv::Mat& from, const cv::Mat &to, cv::Vec3d &tffinal, double radbase, double radstep, int nstep, double updateratio, int nupdate)
	{
		double radstart = radbase;
		double radstepi = radstep;
		double minerr = 256;
		for (int i = 0; i < nupdate; i++)
		{
			for (int i = -nstep; i <= nstep; i++)
			{
				cv::Vec3d w3;
				double radi = radstart + i*radstepi;
				double err = irerr(radi, from, to, w3);
				if (err < minerr)
				{
					minerr = err;
					tffinal = w3;
					std::cout << "update\n";
				}
			}
			radstart = tffinal[2];
			//radstepi *= updateratio;
			radstepi /= nstep - 1;
		}
		return minerr;
	}


	static bool FusionOffset3(cv::Mat &outImage, cv::Mat& inImage1, cv::Rect inRoi1, cv::Mat &inImage2, cv::Rect inRoi2,
		double radbase, double radstep, int nstep, double updateratio, int nupdate)
	{
		if (inImage1.channels() != inImage2.channels())
			return false;

		cv::Mat inImage1f;
		if (inImage1.channels() != 1)
		{
			cv::Mat gray;
			cv::cvtColor(inImage1(inRoi1), gray, CV_BGR2GRAY);
			gray.convertTo(inImage1f, CV_32F);
		}
		else
		{
			inImage1(inRoi1).convertTo(inImage1f, CV_32F);
		}

		cv::Mat inImage2f;
		if (inImage2.channels() != 1)
		{
			cv::Mat gray;
			cv::cvtColor(inImage2(inRoi2), gray, CV_BGR2GRAY);
			gray.convertTo(inImage2f, CV_32F);
		}
		else
		{
			inImage2(inRoi2).convertTo(inImage2f, CV_32F);
		}

		cv::Vec3d w3;
		double err = tune(inImage1f, inImage2f, w3, radbase, radstep, nstep, updateratio, nupdate);

		cv::Matx22d rm(cos(w3[2]), -sin(w3[2]), sin(w3[2]), cos(w3[2]));
		cv::Vec2d tt = rm*cv::Vec2d(-inRoi1.x, -inRoi1.y) + cv::Vec2d(w3[0] + inRoi2.x, w3[1] + inRoi2.y);

		outImage = AlignPage::FeatherFusion2(inImage1, inImage2, cv::Vec3d(tt[0], tt[1], w3[2]));
		return true;
	}

	static bool guessroi(cv::Size fromSize, cv::Size toSize, cv::Vec3d xya, cv::Rect &fromRoi, cv::Rect &toRoi)
	{

		cv::Matx22d rm(cos(xya[2]), -sin(xya[2]), sin(xya[2]), cos(xya[2]));
		cv::Vec2d t(xya[0], xya[1]);

		cv::RotatedRect fromR;
		fromR.size.width = fromSize.width - 1;
		fromR.size.height = fromSize.height - 1;
		fromR.angle = xya[2] * 180 / CV_PI;
		cv::Vec2d ct0 = rm*cv::Vec2d(fromR.size.width, fromR.size.height)*0.5 + t;
		fromR.center.x = ct0[0];
		fromR.center.y = ct0[1];


		cv::RotatedRect toR;
		toR.size.width = toSize.width - 1;
		toR.size.height = toSize.height - 1;
		toR.angle = 0;
		toR.center.x = toR.size.width*0.5;
		toR.center.y = toR.size.height*0.5;


		std::vector<cv::Vec2f> intersectVertexAtTo;
		int res = cv::rotatedRectangleIntersection(fromR, toR, intersectVertexAtTo);

		if (res == cv::INTERSECT_NONE)
			return false;


		std::cout << cv::Mat(intersectVertexAtTo) << "\n";

		toRoi = cv::boundingRect(intersectVertexAtTo);

		toR.angle = -xya[2] * 180 / CV_PI;
		cv::Vec2d ct1 = rm.t()*(cv::Vec2d(toR.size.width, toR.size.height)*0.5 - t);
		toR.center.x = ct1[0];
		toR.center.y = ct1[1];

		fromR.angle = 0;
		fromR.center.x = fromR.size.width*0.5;
		fromR.center.y = fromR.size.height*0.5;

		std::vector<cv::Vec2f> intersectVertexAtFrom;
		res = cv::rotatedRectangleIntersection(fromR, toR, intersectVertexAtFrom);

		if (res == cv::INTERSECT_NONE)
			return false;


		std::cout << cv::Mat(intersectVertexAtFrom) << "\n";

		fromRoi = cv::boundingRect(intersectVertexAtFrom);

		return true;
	}


	static cv::Vec3d GuessWarp2(cv::Point from1, cv::Point to1, cv::Point from2, cv::Point to2)
	{
		cv::Point2d f1d(from1.x, from1.y);
		cv::Point2d t1d(to1.x, to1.y);
		cv::Point2d f2d(from2.x, from2.y);
		cv::Point2d t2d(to2.x, to2.y);

		cv::Matx44d A(f1d.x, -f1d.y, 1, 0,
			f1d.y, f1d.x, 0, 1,
			f2d.x, -f2d.y, 1, 0,
			f2d.y, f2d.x, 0, 1
		);

		cv::Vec4d B(t1d.x, t1d.y, t2d.x, t2d.y);

		cv::Vec4d X = A.inv()*B;

		double theta = atan2(X[1], X[0]);

		return cv::Vec3d(X[2], X[3], theta);
	}

	static void dpdf(const cv::Mat &from, const cv::Mat &to, cv::Vec3d xya)
	{
		cv::Matx23d tf(cos(xya[2]), -sin(xya[2]), xya[0], sin(xya[2]), cos(xya[2]), xya[1]);
		cv::Mat fromr;
		cv::warpAffine(from, fromr, tf, to.size(), cv::INTER_CUBIC, cv::BORDER_CONSTANT, cv::Scalar::all(0));

		cv::Mat adf;
		cv::absdiff(to, fromr, adf);

		cv::namedWindow("adf", cv::WINDOW_NORMAL);
		cv::imshow("adf", adf);
		cv::waitKey(1);

		cv::imwrite("adf.png", adf);
	}


	static bool FusionOffset3(cv::Mat &outImage, cv::Mat& inImage1, cv::Point inPoint1a, cv::Point inPoint1b,
		cv::Mat &inImage2, cv::Point inPoint2a, cv::Point inPoint2b, double radstep = 0.001, int nstep = 5, int nupdate = 10)
	{

		cv::Vec3d w2 = GuessWarp2(inPoint1a, inPoint2a, inPoint1b, inPoint2b);

		//dpdf(inImage1, inImage2, w2);
		std::cout << w2 << "\n";

		cv::Rect rc[2];

		bool res = guessroi(inImage1.size(), inImage2.size(), w2, rc[0], rc[1]);

		if (!res)
			return false;

		std::cout << rc[0] << '\t' << rc[1] << '\n';

		return FusionOffset3(outImage, inImage1, rc[0], inImage2, rc[1], w2[2], radstep, nstep, 0.9, nupdate);
	}


	static bool FusionOffset3(cv::Mat &outImage, cv::Mat& inImage1, cv::Point inPoint1,
		cv::Mat &inImage2, cv::Point inPoint2, double radstep = 0.001, int nstep = 5, int nupdate = 10)
	{
		if (inImage1.channels() != inImage2.channels())
			return false;

		cv::Mat inImage1f;
		if (inImage1.channels() != 1)
		{
			cv::Mat gray;
			cv::cvtColor(inImage1, gray, CV_BGR2GRAY);
			gray.convertTo(inImage1f, CV_32F);
		}
		else
		{
			inImage1.convertTo(inImage1f, CV_32F);
		}

		cv::Mat inImage2f;
		if (inImage2.channels() != 1)
		{
			cv::Mat gray;
			cv::cvtColor(inImage2, gray, CV_BGR2GRAY);
			gray.convertTo(inImage2f, CV_32F);
		}
		else
		{
			inImage2.convertTo(inImage2f, CV_32F);
		}

		double rad = findrotate(inImage1f, inPoint1, inImage2f, inPoint2, false);

		cv::Matx22d rm(cos(rad), -sin(rad), sin(rad), cos(rad));

		cv::Vec2d t = cv::Vec2d(inPoint2.x, inPoint2.y) - rm*cv::Vec2d(inPoint1.x, inPoint1.y);

		cv::Vec3d w2(t[0], t[1], rad);

		//dpdf(inImage1, inImage2, w2);
		std::cout << w2 << "\n";

		cv::Rect rc[2];

		bool res = guessroi(inImage1.size(), inImage2.size(), w2, rc[0], rc[1]);

		if (!res)
			return false;

		std::cout << rc[0] << '\t' << rc[1] << '\n';

		return FusionOffset3(outImage, inImage1, rc[0], inImage2, rc[1], w2[2], radstep, nstep, 0.9, nupdate);
	}
};

